#include "matrix.h"

double &matrix::operator()(unsigned int i, unsigned int j)
{
    double &p = body[i * column + j];
    return p;
}

matrix::matrix(const string init_name, const int init_row, const int init_column)
    : name(init_name), row(init_row), column(init_column)
{
    rank = -1;
    module = -1;
    det = 0;
    this->body.clear();
}

matrix::matrix(const string init_name, const int init_row, const int init_column, const std::initializer_list<double> ini)
    : name(init_name), row(init_row), column(init_column)
{
    rank = -1;
    module = -1;
    det = 0;
    this->body.clear();
    this->body.assign(ini.begin(), ini.end());
}

matrix::matrix(const int init_row, const int init_column)
    : row(init_row), column(init_column)
{
    rank = -1;
    module = -1;
    det = 0;
    this->body.clear();
}

matrix::matrix(const int init_row, const int init_column, const std::initializer_list<double> ini)
    : row(init_row), column(init_column)
{
    rank = -1;
    module = -1;
    det = 0;
    this->body.clear();
    this->body.assign(ini.begin(), ini.end());
}

matrix::matrix(int N)
{
    this->name = "E[" + to_string(N) + "]";
    det = 1, rank = N;
    row = N;
    column = N;
    this->body.clear();
    this->body.assign(N * N, 0);
    for (int i = 0; i < N; i++)
    {
        this->operator()(i, i) = 1;
    }

    // for (int i = 0; i < N; i++)
    // {
    //     for (int j = 0; j < N; j++)
    //     {
    //         if (j - i == 0)
    //         {
    //             this->body.push_back(1);
    //         }
    //         else
    //         {
    //             this->body.push_back(0);
    //         }
    //     }
    // }
}

vector<double> &matrix::Init(const int init_row, const int init_column)
{
    if (init_row * init_column != 0)
    {
        row = init_row;
        column = init_column;
        rank = -1, det = 0, module = -1;
        body.clear();

        // body.resize(init_row);
        // for (int i = 0; i < init_row; i++)
        // {
        //     body[i].resize(init_column);
        // }

        return body;
    }
    else
    {
        cout << "\nWrong with error:-1 [Error]:Invalid object!\n";
        return body;
    }
}

vector<double> &matrix::Init(const string init_name, const int init_row, const int init_column)
{
    if (init_row * init_column != 0)
    {
        body.clear();
        name = init_name;
        row = init_row;
        column = init_column;
        module = -1, rank = -1, det = 0;
        return body;

        // body.resize(init_row);
        // for (int i = 0; i < init_row; i++)
        // {
        //     body[i].resize(init_column);
        // }
    }
    else
    {
        cout << "\nWrong with error:-1 [Error]:Invalid object!\n";
        return body;
    }
}

void matrix::Stock_byColumn(void)
{
    if (!body.empty())
    {
        string name_temp = this->name;
        int row_temp = this->row;
        this->row = this->column;
        this->column = row_temp;
        (*this) = ~(*this);
        this->name = name_temp;
    }
    else
    {
        cout << "\nWrong with error:-1 [Error]:Empty object!\n";
    }
}

// void R_3d::Boost(double alpha, double beta, double gramma)
// {
//     row = 3, column = 3;
//     theta_x = alpha, theta_y = beta, theta_z = gramma;
//     matrix Rx, Ry, Rz;
//     Rx.Init(3, 3) = {1, 0, 0, 0, cos(theta_x), -sin(theta_x), 0, sin(theta_x), cos(theta_x)};
//     Ry.Init(3, 3) = {cos(theta_y), 0, -sin(theta_y), 0, 1, 0, sin(theta_y), 0, cos(theta_y)};
//     Rz.Init(3, 3) = {cos(theta_z), -sin(theta_z), 0, sin(theta_z), cos(theta_z), 0, 0, 0, 1};
//     this->body = (Rx * Ry * Rz).body;
// }

// void R_2d::Boost(double alpha)
// {
//     row = 2, column = 2;
//     theta_x = alpha;
//     this->body = {cos(theta_x), -sin(theta_x), sin(theta_x), cos(theta_x)};
// }

void matrix::Head(void) const
{
    cout << name << ' ' << '<' << row << ',' << column << '>' << '\n';

    // To get the variable type
    // string tpid = typeid(*this).name();
    // string::iterator itr = tpid.begin();
    // tpid.erase(itr);
    // tpid = "[" + tpid + "]: ";
    // cout << tpid;
}

void matrix::Out(const char key) const
{
    if (!body.empty())
    {
        Head();
        if (key == 'f') // Print in format.
        {
            for (int i = 0; i < this->row; i++)
            {
                for (int j = 0; j < this->column; j++)
                {
                    if (j != 0)
                        printf("%.3g\t", this->body[(i) * this->column + j]);
                    else
                        printf("[ %.3g\t", this->body[(i) * this->column + j]);
                }
                printf("]\n");
            }
        }
        else // Print without format.
        {
            for (int i = 0; i < this->row; i++)
            {
                for (int j = 0; j < this->column; j++)
                {
                    if (j != 0)
                        printf(" %.3g ", this->body[(i) * this->column + j]);
                    else
                        printf("[ %.3g ", this->body[(i) * this->column + j]);
                }
                printf("]\n");
            }
        }
        cout << '\n';
    }
    else
    {
        cout << "\nWrong with error:-1 [Error]:Empty object!\n";
    }
}

void matrix::Out(void) const
{
    if (!body.empty())
    {
        Head();
        for (int i = 0; i < this->row; i++)
        {
            for (int j = 0; j < this->column; j++)
            {
                if (j != 0)
                    printf(" %g ", this->body[(i) * this->column + j]);
                else
                    printf("[ %g ", this->body[(i) * this->column + j]);
            }
            printf("]\n");
        }

        cout << '\n';
    }
    else
    {
        cout << "\nWrong with error:-1 [Error]:Empty object!\n";
    }
}

void matrix::Clear(void)
{
    *this = NULL_MATRIX;
}

matrix Approx(matrix &&Mt)
{
    for (int i = 0; i < Mt.row * Mt.column; i++)
    {
        if (fabs(Mt.body[i]) <= LIMIT_ZERO)
        {
            Mt.body[i] = 0;
        }
    }
    Mt.name = "(r#" + Mt.name + ")";
    return Mt;
}

matrix &matrix::operator=(const matrix &obj)
{
    this->body.clear();
    for (int i = 0; i < obj.row * obj.column; i++)
    {
        this->body.push_back(obj.body[i]);
    }
    return *this;
}

bool matrix::operator==(const matrix &latter)
{
    int check = 0;
    const int check_rate = 9; // Can be modified.

    for (int i = 0; i < this->body.size(); i++)
    {
        if (fabs(this->body[i] - latter.body[i]) <= 100 * LIMIT_ZERO)
            check++;
    }

    if (check >= (int)(this->body.size() * 0.1 * check_rate))
        return true;
    else
        return false;
}

matrix matrix::operator+(const matrix &latter)
{
    if (this->row == latter.row && this->column == latter.column &&
        (this->row * this->column != 0) &&
        (latter.row * latter.column != 0))
    {
        matrix add_result("(" + (this->name) + "+" + latter.name + ")", this->row, this->column);
        for (int i = 0; i < latter.row * latter.column; i++)
        {
            add_result.body.push_back((this->body)[i] + latter.body[i]);
        }
        return add_result;
    }
    else
    {
        cout << "Wrong with error:-2 [Error]:Row or column not match!\n";
        return NULL_MATRIX;
    }
}

matrix matrix::operator-(const matrix &latter)
{
    if (this->row == latter.row && this->column == latter.column &&
        (this->row * this->column != 0) &&
        (latter.row * latter.column != 0))
    {
        matrix minus_result(this->row, this->column);
        minus_result = (*this) + (-1 * latter);
        minus_result.name = "(" + (this->name) + "-" + latter.name + ")";
        return minus_result;
    }
    else
    {
        cout << "Wrong with error:-2 [Error]:Row or column not match!\n";
        return NULL_MATRIX;
    }
}

// vct getColumn(matrix Mt, const int c)
// {
//     if (c < Mt.column)
//     {
//         vct a;
//         a.Init("getColumn{}", Mt.row, 1);
//         a.name = Mt.name + "[" + to_string(c) + "]";
//         a.body.clear();
//         for (int m = 0; m < Mt.row; m++)
//         {
//             a.body.push_back(Mt(m, c));
//         }
//         return a;
//     }
//     else
//     {
//         vct NULL_matrix;
//         NULL_matrix.Clear();
//         cout << "Wrong with error:-1\n";
//         return NULL_matrix;
//     }
// }

// matrix matrix::operator[](const int c)
// {
//     return getColumn(*this, c);
// }

// vct getRow(matrix Mt, const int r)
// {
//     if (r < Mt.row)
//     {
//         vct a;
//         a.Init("getRow{}", 1, Mt.column);
//         a.name = Mt.name + ".getRow{" + to_string(r) + "}";
//         a.body.clear();

//         for (int m = 0; m < Mt.column; m++)
//         {
//             a.body.push_back(Mt(r, m));
//         }
//         return a;
//     }
//     else
//     {
//         vct NULL_matrix;
//         NULL_matrix.Clear();
//         cout << "Wrong with error:-1\n";
//         return NULL_matrix;
//     }
// }

matrix matrix::operator*(const matrix &second)
{
    if (this->column == second.row)
    {
        matrix multi_result("(" + (this->name) + "*" + second.name + ")", this->row, second.column);
        multi_result.body.clear();

        // for (int i = 0; i < multi_result.row; i++)
        // {
        //     for (int j = 0; j < multi_result.column; j++)
        //     {
        //         multi_result.body.push_back(0);
        //     }
        // }

        for (int i = 0; i < this->row; i++)
        {
            for (int j = 0; j < second.column; j++)
            {
                double temp = 0;
                for (int c = 0; c < this->column; c++)
                {
                    // multi_result.body[(i)*multi_result.column + j] +=
                    //     (this->body[(i) * (this->column) + c] *
                    //      second.body[(c)*second.column + j]);

                    temp += (this->body[(i) * (this->column) + c] *
                             second.body[(c)*second.column + j]);
                }
                multi_result.body.push_back(temp);
            }
        }
        return multi_result;
    }
    else
    {
        cout << "Wrong with error:-2 [Error]: A<i,k> * B<k,j> = C<i,j>\n";
        return NULL_MATRIX;
    }
}

matrix Power(const matrix Mt, unsigned int pow)
{
    if (Mt.row == Mt.column)
    {
        if (pow > 0)
        {
            matrix result;
            string temp_name = Mt.name;
            result.row = Mt.row;
            result.column = Mt.column;
            result.body = Mt.body;
            for (int i = 1; i < pow; i++)
            {
                result = result * (Mt);
            }
            result.name = "(" + temp_name + '^' + to_string(pow) + ")";
            return result;
        }
        else
        {
            if (pow == 0)
            {
                matrix result(Mt.row);
                result.name = "(" + Mt.name + '^' + to_string(pow) + ")";
                return result;
            }
            else
            {
                return Inv(Mt);
            }
        }
    }
    else
    {
        cout << "\nWrong with error:-2 [Error]: Not a cube!\n";
        return NULL_MATRIX;
    }
}

matrix matrix::operator^(const int pow)
{
    if (row == column)
    {
        switch (pow)
        {
        case -1:
            return Inv(*this);
            break;
        default:
            return Power(*this, pow);
            break;
        }
    }
    else
    {
        return NULL_MATRIX;
    }
}

matrix Tran(matrix Mt)
{
    matrix result("(~" + Mt.name + ")", Mt.column, Mt.row);
    result.body = Mt.body;
    for (int i = 0; i < result.row; i++)
    {
        for (int j = 0; j < result.column; j++)
        {
            result(i, j) = Mt(j, i);
        }
    }
    return result;
}

matrix matrix::operator~(void)
{
    matrix result;
    return Tran(*this);
}

double Module(vct va)
{
    if (va.row == 1 || va.column == 1)
    {
        double sum = 0;
        for (int i = 0; i < va.row * va.column; i++)
        {
            sum += va.body[i] * va.body[i];
        }
        return sqrt(sum);
    }
    else
    {
        cout << "Wrong with error:-2 [Error]: Not a vector!\n";
        return EOF;
    }
}

double Dot(vct va, vct vb)
{
    if (va.row * va.column == vb.row * vb.column && (va.row == 1 || va.column == 1) && (vb.row == 1 || vb.column == 1))
    {
        double ans;
        for (int i = 0; i < (va.row * va.column); i++)
        {
            ans += (va.body[i]) * (vb.body[i]);
        }
        return ans;
    }
    else
    {
        cout << "Wrong with error:-2 [Error]:Invalid calculation!\n";
        return EOF;
    }
}

vct Cross(vct va, vct vb)
{
    if ((va.row * va.column == 3) && (vb.row * vb.column == 3))
    {
        vct cross_result('(' + va.name + "_x_" + vb.name + ')', 3, 1);
        cross_result.body.push_back(va.body[1] * vb.body[2] - va.body[2] * vb.body[1]);
        cross_result.body.push_back(va.body[2] * vb.body[0] - va.body[0] * vb.body[2]);
        cross_result.body.push_back(va.body[0] * vb.body[1] - va.body[1] * vb.body[0]);

        return cross_result;
    }
    else
    {
        cout << "Wrong with error:-2 [Error]: Invalid operation!\n";
        return NULL_MATRIX;
    }
}

double Angle(vct va, vct vb)
{
    if (va.row * va.column == vb.row * vb.column && (va.row == 1 || va.column == 1) && (vb.row == 1 || vb.column == 1))
    {
        double theta, dot, mod;
        mod = Module(va) * Module(vb);
        dot = Dot(va, vb);
        theta = acos(dot / mod);
        return theta;
    }
    else
    {
        cout << "Wrong with error:-2 [Error]:Invalid calculation!\n";
        return EOF;
    }
}

// double standard_echelon(double matrix[_MAX_SCALE][_MAX_SCALE], int r, int c, int x, int y)
// {
//     int i, j, k, l, total[_MAX_SCALE] = {0};
//     double times, temp, result = 0, original_matrix[_MAX_SCALE][_MAX_SCALE];

//     for (i = 0; i < r; i++)
//         for (j = 0; j < c; j++)
//             original_matrix[i][j] = matrix[i][j];
//     for (i = 0; i < r - 1; i++)
//         for (k = i + 1; k < r; k++)
//         {
//             j = 0;
//             while (matrix[i][j] == 0)
//                 j++;
//             if (matrix[i][j] != 0)
//             {
//                 times = matrix[k][j] / matrix[i][j];
//                 for (j = 0; j < c; j++)
//                     matrix[k][j] -= matrix[i][j] * times;
//             }
//         }
//     for (i = 0; i < r; i++)
//     {
//         j = 0;
//         while (matrix[i][j] == 0)
//             j++;
//         if (matrix[i][j] != 0)
//         {
//             times = matrix[i][j];
//             for (j = 0; j < c; j++)
//                 matrix[i][j] /= times;
//         }
//     }
//     for (i = 0; i < r; i++)
//         for (j = 0; j < c; j++)
//             if (matrix[i][j] == 0)
//                 total[i]++;
//             else
//                 break;
//     for (l = r - 1; l > 0; l--)
//         for (i = 0; i < l; i++)
//             if (total[l] < total[i])
//                 for (j = 0; j < c; j++)
//                 {
//                     temp = matrix[l][j];
//                     matrix[l][j] = matrix[i][j];
//                     matrix[i][j] = temp;
//                 }
//     for (i = 0; i < r; i++)
//     {
//         j = 0;
//         while (matrix[i][j] == 0)
//             j++;
//         if (matrix[i][j] != 0)
//             for (k = 0; k < i; k++)
//             {
//                 times = matrix[k][j] / matrix[i][j];
//                 for (l = 0; l < c; l++)
//                     matrix[k][l] -= times * matrix[i][l];
//             }
//     }
//     result = matrix[x][y];
//     for (i = 0; i < r; i++)
//         for (j = 0; j < c; j++)
//             matrix[i][j] = original_matrix[i][j];
//     if (fabs(result) <= LIMIT_ZERO)
//         result = 0;

//     return result;
// }

matrix Echelon(matrix Mt)
{
    // matrix result = Mt;
    // result.name = "(e#" + Mt.name + ")";

    // double TEMP_matrix[_MAX_SCALE][_MAX_SCALE] = {0};

    // for (int i = 0; i < result.row; i++)
    // {
    //     for (int j = 0; j < result.column; j++)
    //     {
    //         TEMP_matrix[i][j] = Mt(i, j);
    //     }
    // }

    // for (int i = 0; i < result.row; i++)
    // {
    //     for (int j = 0; j < result.column; j++)
    //     {
    //         result(i, j) = standard_echelon(TEMP_matrix, result.row, result.column, i, j);
    //     }
    // }
    // return result;

    for (int i = 0; i < Mt.row - 1; i++)
    {
        // 找主元
        int pos = 0;
        for (int j = 0; j < Mt.column; j++)
            if ((Mt(i, j)))
            {
                pos = j;
                break;
            }

        if (!isZERO(Mt(i, pos) - 1) && !isZERO(Mt(i, pos)))
        {
            long double tmp = Mt(i, pos);
            for (int j = pos; j < Mt.column; j++)
            {
                Mt(i, j) = Mt(i, j) / tmp;
            }
        }
        for (int j = i + 1; j < Mt.row; j++)
        {
            if (isZERO(Mt(i, pos)))
                continue;
            long double tmp = Mt(j, pos);
            for (int k = pos; k < Mt.column; k++)
            {
                Mt(j, k) = Mt(j, k) - Mt(i, k) * tmp;
            }
        }
    }

    // 向后步骤
    for (int i = Mt.row - 1; i > 0; i--)
    {
        int pos = 0;
        for (int j = 0; j < Mt.column; j++)
            if (Mt(i, j))
            {
                pos = j;
                break;
            }

        if (!isZERO(Mt(i, pos) - 1) && !isZERO(Mt(i, pos)))
        {
            long double tmp = Mt(i, pos);
            for (int j = pos; j < Mt.column; j++)
            {
                Mt(i, j) = Mt(i, j) / tmp;
            }
        }

        for (int j = 0; j < i; j++)
        {
            if (isZERO(Mt(j, pos)))
                continue;
            long double tmp = Mt(j, pos);
            for (int k = pos; k < Mt.column; k++)
            {
                Mt(j, k) = Mt(j, k) - Mt(i, k) * tmp;
            }
        }
    }

    return Mt;
}

int Rank(matrix Mt)
{
    int none_zero_element = 0, result = 0;
    matrix TEMP = Echelon(Mt);
    for (int i = 0; i < Mt.row; i++)
    {
        for (int j = 0; j < Mt.column; j++)
        {
            if (TEMP(i, j) != 0)
            {
                none_zero_element = 1;
                break;
            }
        }
        if (none_zero_element > 0)
        {
            result++;
        }
        none_zero_element = 0;
    }
    return result;
}

matrix Inv(matrix Mt)
{
    if (Mt.row == Mt.column && Mt.column == Rank(Mt))
    {
        matrix TEMP(Mt.row, Mt.column * 2);

        for (int i = 0; i < Mt.row; i++) // Create (Mt:E[n])
        {
            for (int j = 0; j < Mt.column * 2; j++)
            {
                if ((j - i) == Mt.row)
                {
                    TEMP.body.push_back(1);
                }
                else
                {
                    TEMP.body.push_back(0);
                }
            }
        }

        for (int i = 0; i < Mt.row; i++)
        {
            for (int j = 0; j < Mt.column; j++)
            {
                TEMP(i, j) = Mt(i, j);
            }
        }

        matrix result = Mt;
        matrix combi = Echelon(TEMP);

        for (int i = 0; i < Mt.row; i++)
        {
            for (int j = Mt.column; j < Mt.column * 2; j++)
            {
                result(i, j - Mt.column) = (combi)(i, j);
            }
        }
        result.name = "(!" + Mt.name + ")";
        return result;
    }
    else
    {
        if (Mt.row != Mt.column)
        {
            cout << "Wrong with error:-2 [Error]: Not a cube!\n";
            return NULL_MATRIX;
        }
        else
        {
            cout << "Wrong with error:-2 [Error]: Invalid operation!\n";
            return NULL_MATRIX;
        }
    }
}

matrix matrix::operator!(void)
{
    matrix result;
    return Inv(*this);
}

matrix Merge_inRow(matrix A, matrix B)
{
    if (A.row == B.row && (!A.body.empty()) && (!B.body.empty()))
    {
        matrix result;
        result.name = "(" + A.name + ":" + B.name + ")";
        result.row = A.row;
        result.column = A.column + B.column;

        for (int i = 0; i < result.row; i++)
        {
            for (int j = 0; j < result.column; j++)
            {
                if (j + 1 <= A.column)
                    result.body.push_back(A(i, j));
                else
                    result.body.push_back(B(i, j - A.column));
            }
        }

        return result;
    }
    else
    {
        cout << "Wrong with error:-2 [Error]: Invalid operation!\n";
        return NULL_MATRIX;
    }
}

matrix Merge_inColumn(matrix A, matrix B)
{
    if ((A.column == B.column) && (!A.body.empty()) && (!B.body.empty()))
    {
        matrix result;
        result.name = "(" + A.name + "\\" + B.name + ")";
        result.row = A.row + B.row;
        result.column = A.column;

        for (int i = 0; i < result.row; i++)
        {
            for (int j = 0; j < result.column; j++)
            {
                if (i + 1 <= A.row)
                    result.body.push_back(A(i, j));
                else
                    result.body.push_back(B(i - A.row, j));
            }
        }

        return result;
    }
    else
    {
        cout << "Wrong with error:-2 [Error]: Invalid operation!\n";
        return NULL_MATRIX;
    }
}

double determinant(double matrix[_MAX_SCALE][_MAX_SCALE], int order)
{
    int sign = 1, i;
    double result = 0, cofactor;
    if (order == 1)
    {
        result = matrix[0][0];
    }
    else
    {
        for (i = 0; i < order; i++)
        {
            cofactor = laplace_expansion(matrix, i, 0, order);
            result += sign * matrix[i][0] * cofactor;
            sign *= -1;
        }
    }

    return result;
}

double laplace_expansion(double matrix[_MAX_SCALE][_MAX_SCALE], int r, int c, int order)
{
    double result = 0, cofactor[_MAX_SCALE][_MAX_SCALE];
    int original_i, original_j, i, j;

    for (i = 0; i < order; i++)
    {
        for (j = 0; j < order; j++)
        {
            original_i = i;
            original_j = j;
            if (i == r || j == c)
                ;
            else
            {
                if (i > r)
                    i--;
                if (j > c)
                    j--;
                cofactor[i][j] = matrix[original_i][original_j];
                i = original_i;
                j = original_j;
            }
        }
    }

    if (order >= 2)
        result = determinant(cofactor, order - 1);

    return result;
}

double Det(matrix Mt)
{
    if (Mt.row == Mt.column)
    {
        double TEMP_matrix[_MAX_SCALE][_MAX_SCALE];

        for (int i = 0; i < Mt.row; i++)
        {
            for (int j = 0; j < Mt.column; j++)
            {
                TEMP_matrix[i][j] = Mt(i, j);
            }
        }
        return determinant(TEMP_matrix, Mt.row);
    }
    else
    {
        cout << "Wrong with error:-2 [Error]: Not a cube!\n";
        return EOF;
    }
}

// identity_matrix::identity_matrix(int N)
// {
//     this->name = "E[" + to_string(N) + "]";
//     det = 1, rank = N;
//     row = N;
//     column = N;
//     for (int i = 0; i < N; i++)
//     {
//         for (int j = 0; j < N; j++)
//         {
//             if (j - i == 0)
//             {
//                 this->body.push_back(1);
//             }
//             else
//             {
//                 this->body.push_back(0);
//             }
//         }
//     }
// }

void matrix::Info(void)
{
    if (this->row == 1 || this->column == 1)
    {
        this->module = Module(*this);
    }
    else
    {
        if (this->row == this->column)
        {
            this->det = Det(*this);
            this->rank = Rank(*this);
        }
        else
        {
            this->rank = Rank(*this);
        }
    }
}

bool isZERO(double _X)
{
    if (fabs(_X) <= 100.0 * LIMIT_ZERO)
        return true;
    else
        return false;
}